#!/usr/bin/env python
#coding=utf-8
import sys
from time import time
#from genpy import message
#from manipulator_setup import handle_mode_set, manipulator_node
import rospy
from birl_manipulator.srv import *
from birl_manipulator.msg import *
from rospkg import RosPack
from rospy.exceptions import ROSException, ROSInterruptException
robot_record_file=RosPack().get_path('birl_manipulator')+"/file/joint_record.txt"
file= open(robot_record_file,'a+')

# 订阅者的回调函数，在该函数中完成关节角的记录工作
def callback(data):
    rospy.loginfo(data.feedbackPosData)
    for i in range(len(data.feedbackPosData)):
        if i!=4:
            file.write('%s'%data.feedbackPosData[i]+' ')
        else:
            file.write('%s'%data.feedbackPosData[i])
            file.write('\n')
    

def listener():
    rospy.init_node("manipulator_listener_node",anonymous=True)
    rospy.Subscriber("manipulator_position",robot_feedback,callback)
    rospy.spin()

if __name__=="__main__":
    try:
        listener()
    except ROSInterruptException as e:
        file.close()